#include "modules/CtrlModule/VelPubModule.h"
#include "modules/AssistModule/LogModule.h"

namespace behavior_controller
{
VelPubModule vel_pub_mod;

VelPubModule::VelPubModule(const rclcpp::Node::SharedPtr &ros_node) : ros_node_(ros_node)
{
    vel_pub_ = ros_node_->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
    LogPub::Info("behavior_controller", "[VelPubModule] ...................VelPubModule::VelPubModule");
};

void VelPubModule::SetNode(const rclcpp::Node::SharedPtr &ros_node)
{
    if (vel_pub_ == nullptr)
    {
        ros_node_ = ros_node;
        vel_pub_ = ros_node_->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
        LogPub::Info("behavior_controller", "[VelPubModule] ...................VelPubModule::SetNode");
    }
};

void VelPubModule::VelPub(double linear, double angular)
{
    geometry_msgs::msg::Twist cmd_vel;
    //线速度限幅：
    if (linear > 1.0f)
        cmd_vel.linear.x = 1.0f;
    else if (linear < -1.0f)
        cmd_vel.linear.x = -1.0f;
    else
        cmd_vel.linear.x = linear;
    //角速度限幅：
    if (angular > 1.0f)
        cmd_vel.angular.z = 1.0f;
    else if (angular < -1.0f)
        cmd_vel.angular.z = -1.0f;
    else
        cmd_vel.angular.z = angular;
    vel_pub_->publish(cmd_vel);

}
} // namespace behavior_controller
